/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/opSrc/operation.cpp
 * @Description  : 定义了Operation类及其相关方法，用于处理AGV相关操作。
 *                 Operation类负责管理操作的生命周期，包括初始化、准备开始、执行、工作、准备结束和结束操作等阶段。
 *                 该类还提供了检查二维码ID匹配、强制停止操作、检查操作完成状态等功能。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:28:27
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "operation.h" 
#include "tools.h"
#include "public_kernel.h"
#include <tf2/utils.h> 
#include <array>
//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "Operation".
// #1  blockingtype:
//  Enum {NOTE, SOFT, HARD}:
//  "NONE"- allows driving and other actions;
//  "SOFT"- allows other actions, but not driving;
//  "HARD"- is the only allowed action at that time.

/**
 * @brief Operation类的构造函数
 *
 * 初始化Operation对象，包括总计数、阶段、操作和行为参数等。
 *
 * @param action 操作消息，包含操作类型和行为参数
 */
Operation::Operation(const agv_msgs::msg::Action& action) 
    :total_count_(0),
    stage_(OpInit),
    action_(action),
    delay_count_(0),
    first_delay_count_(0)                
{
    std::string type = action_.blockingtype;
    if (type == DYN_TYPE) {
        dyn_op_ = 0xFFFF; // NULLID
        std::vector<agv_msgs::msg::ActionParam> params = action_.actionparameters;
        for (auto iter : params) {
            if (iter.key == "endNode") {
                if (!string2Number(iter.value, dyn_op_)) {
                    DEBUG_ERROR_OUT("error: dyn op end node failed: value:" << iter.value);
                } else {
                    DEBUG_OUT("dyn op end node: value:" << dyn_op_);
                }
                break;
            }
        }
    }

    FindQrId();
    timer_trigger_.Stop();
    DEBUG_WARN_OUT("blockingtype=" << type); 
}

/**
 * @brief 周期性的操作执行函数
 *
 * 根据当前操作阶段（stage_）执行相应的操作，并在每个阶段完成时输出调试信息。
 *
 * 如果某个阶段的操作失败，则不会进入下一个阶段，停留在当前阶段。
 *
 * 如果连续执行超过20次，输出当前操作阶段信息。
 */
void Operation::CycleRoutine()
{
    switch (stage_) {
        case OpInit:
            if (Init()) {
                ResetOpValue();
                stage_ = OpReadyStart;
                DEBUG_OUT("op run stage: enter OpReadyStart");
            }
            break;
        case OpReadyStart:
            if (ReadyStart()) {
                stage_ = OpStart;
                DEBUG_OUT("op run stage: enter OpStart");
            }
            break;
        case OpStart:
            if (StartAction()) {
                stage_ = OpWork;
                DEBUG_OUT("op run stage: enter OpWork");
            }
            break;
        case OpWork:
            if (Work()) {
                stage_ = OpReadyEnd;
                DEBUG_OUT("op run stage: enter OpReadyEnd");
            }
            break;
        case OpReadyEnd:
            if (EndAction()) {
                stage_ = OpEnd;
                DEBUG_OUT("op run stage: enter OpEnd");
            }
            break;
        case OpEnd:
            DEBUG_OUT("op run stage: OpEnd");
            break;
        case OpError:
            DEBUG_OUT("action do error! op run stage: OpError");
            break;
        default:
            break;
    }

    static int times = 0;
    if (times++ > 20) {
        times = 0;
        DEBUG_OUT("cur op CurStage:" << GetCurStage());
    }
}

/**
 * @brief 强制停止操作
 *
 * 尝试强制停止当前操作，并立即结束当前动作。
 *
 * @return 如果操作成功被强制停止，则返回true；否则返回false。
 */
bool Operation::ForceStop()
{
    DEBUG_WARN_OUT("Operation:try to ForceStop and end action once");
    EndAction();
    stage_ = OpEnd; // FIXME:  important
    return true;
}

/**
 * @brief 判断操作是否完成
 *
 * 检查当前操作是否已完成。
 *
 * @return 如果操作已完成，则返回true；否则返回false。
 */
bool Operation::Finished()
{
    return stage_ == OpEnd;
}

/**
 * @brief 初始化函数
 *
 * 该函数用于初始化Operation对象，进行必要的设置或准备工作。
 *
 * @return 如果初始化成功，则返回true；否则返回false。
 */
bool Operation::Init()
{
    return true;
}

/**
 * @brief 判断操作是否已准备好开始
 *
 * 检查当前操作是否满足开始执行的条件。
 *
 * @return 如果操作已准备好开始执行，则返回true；否则返回false。
 */
bool Operation::ReadyStart()
{
    return true;
}

/**
 * @brief 开始执行操作
 *
 * 启动一个操作，返回操作是否成功启动的标志。
 *
 * @return 如果操作成功启动，则返回true；否则返回false。
 */
bool Operation::StartAction()
{
    return true;
}

/**
 * @brief 执行操作
 *
 * 根据条件决定是否继续工作
 *
 * @return 如果继续工作则返回 true，否则返回 false
 */
bool Operation::Work()
{
#ifdef AGV_DEBUG_NAV
    return m_uTotalCount++ > 100; // 5s-500
#endif
    return true;
}

/**
 * @brief 获取动态操作编号
 *
 * 获取当前动态操作编号。
 *
 * @return 返回动态操作编号
 */
int Operation::GetDynOpEndNode()
{
    return dyn_op_;
}

/**
 * @brief 结束操作
 *
 * 结束当前操作，并返回操作是否成功结束的标志。
 *
 * @return 返回 true 表示操作成功结束，返回 false 表示操作未成功结束。
 */
bool Operation::EndAction()
{
    return true;
}

/**
 * @brief 获取当前阶段
 *
 * 获取当前操作所处的阶段。
 *
 * @return 返回当前阶段，类型为SHORT
 */
SHORT Operation::GetCurStage()
{
    return stage_;
}

/**
 * @brief 重置操作值
 *
 * 该函数用于重置操作值，停止计时器触发。
 */
void Operation::ResetOpValue()
{
    timer_trigger_.Stop();
    DEBUG_OUT("ResetOpValue once...");
}

/**
 * @brief 获取当前操作对象
 *
 * 返回当前操作对象，类型为 agv_msgs::msg::Action
 *
 * @return 返回当前操作对象
 */
agv_msgs::msg::Action Operation::GetCurActionObj()
{
    return action_;
}

/**
 * @brief 查找二维码ID并解析
 *
 * 从action_参数中查找名为NAME_KEY_ACTION_QR_ID和NAME_KEY_ACTION_ENABLE_QR_ID的参数，
 * 并将对应的值解析为bool和int类型，存储到mark_verify_结构体中。
 *
 * @return 如果找到并成功解析了这两个参数，则返回true；否则返回false。
 */
bool Operation::FindQrId()
{
    std::vector<agv_msgs::msg::ActionParam> params = action_.actionparameters;
    std::array<bool, 2> find = {false, false};
    for (auto iter : params) {

        if (iter.key == NAME_KEY_ACTION_QR_ID) {
            if (!string2Number(iter.value, mark_verify_.node_qr_id)) {
                DEBUG_ERROR_OUT("error: node_qr_id failed: value:" << iter.value);
            } else {
                find[0] = true;
                DEBUG_OUT("node_qr_id: value:" << mark_verify_.node_qr_id);
            }
        } else if (iter.key == NAME_KEY_ACTION_ENABLE_QR_ID) {
            if (!string2Number(iter.value, mark_verify_.enable)) {
                DEBUG_ERROR_OUT("error: mark_verify_.enable failed: value:" << iter.value);
            } else {
                find[1] = true;
                DEBUG_OUT("node_qr_id: value:" << mark_verify_.node_qr_id);
            }
        }
    }

    DEBUG_OUT("FindQrId: node_qr_id:" << mark_verify_.node_qr_id
                                    << ",enable=" << mark_verify_.enable
                                    << ",find[0]=" << find[0] << ",find[1]=" << find[1]);

    return find[0] && find[1];
}

/**
 * @brief 检查二维码ID是否匹配
 *
 * 该函数用于检查当前操作的二维码ID是否与预期匹配。
 *
 * @param err_msg 错误信息字符串的引用，用于返回错误信息
 *
 * @return 如果二维码ID匹配，则返回true；否则返回false，并通过err_msg返回错误信息
 */
bool Operation::CheckMarkQrId(std::string &err_msg)
{
    // std::string err_msg = "";
    bool op = OP_MUTEX_TYPE == action_.blockingtype;
    if (!op) {
        DEBUG_OUT("CheckMarkQrId: check ok; cur is not static op: type=" << action_.blockingtype);
        return true;
    }

    if (0 == mark_verify_.enable) {
        DEBUG_OUT("CheckMarkQrId: check ok;(enable is false); node_qr_id=" << mark_verify_.node_qr_id);
        return true;
    }

    if (-1 == mark_verify_.node_qr_id) {
        DEBUG_OUT("CheckMarkQrId: check ok;(cur node is not qrNode);");
        return true;
    }

    const auto &op_data = GetOpData();
    auto ground_camera_pose = op_data.ground_camera_pose_msg;
    //  判断数据的有效性，如果超时 则为无效数据
    if (IsTimeOut(ground_camera_pose.header.stamp, ROSTime(), 600)) {
        DEBUG_ERROR_OUT("CheckMarkQrId:check failed; get landmark time out");
        err_msg = "landmark time out;can not find qr code id;" + std::to_string(mark_verify_.node_qr_id);
        // SetLocMissEvent(err_msg, true);
        return false;
    }

    geometry_msgs::msg::Pose data_pos = ground_camera_pose.pose;
    float data_x = data_pos.position.x;
    float data_y = data_pos.position.y;
    float tag = data_pos.position.z;
    float data_angle = 0.0f;
    try {
        data_angle = tf2::getYaw(data_pos.orientation);
    } catch (...) {
        DEBUG_ERROR_OUT("CheckMarkQrId:tf2::getYaw(data_pos.orientation) failed");
    }

    int cur_tag = tag;
    DEBUG_OUT("CheckMarkQrId:tag=" << tag << ",x=" << data_x << ",y=" << data_y << ",t=" << data_angle
                                    << ",cur_tag=" << cur_tag);

    if (cur_tag != mark_verify_.node_qr_id) {
        DEBUG_ERROR_OUT("CheckMarkQrId:failed;cur_tag!=node_qr_id; cur_tag=" << cur_tag
                                                                        << ",node_qr_id=" << mark_verify_.node_qr_id);

        err_msg = "current qr code id is different;curId=" + std::to_string(cur_tag) + ",targetId=" + std::to_string(mark_verify_.node_qr_id);
        // SetLocMissEvent(err_msg, true);
        return false;
    }

    DEBUG_OUT("CheckMarkQrId:normal ok;" << ",cur_tag=" << cur_tag
                                         << ",node_qr_id=" << mark_verify_.node_qr_id);

    return true;
}

/**
 * @brief 设置位置丢失事件
 *
 * 设置AGV的位置丢失事件，并指定该事件是否发生。
 *
 * @param msg 事件描述信息
 * @param occur 事件是否发生，true表示发生，false表示未发生
 */
void Operation::SetLocMissEvent(std::string msg, bool occur)
{
    SetTaskError(agv_msgs::msg::AGVEvent::LOCATION_MISSING_EVENT, msg, agv_msgs::msg::AGVEvent::ESTOP, occur);
}

/**
 * @brief 尝试周期性地检查标记
 *
 * 该函数尝试周期性地检查标记，如果超过2秒且未发现相同二维码或无数据，则报错。
 *
 * @return 如果找到标记，则返回true；否则返回false。
 */
bool Operation::TryCycleMarkCheck() 
{
    std::string err_msg = "";
    bool find = CheckMarkQrId(err_msg);
    if (!find)
    {
        timer_trigger_.Start();
        // 超过2s且未发现相同二维码或无数据 则报错
        if (timer_trigger_.TriggerAlways(2000))
        {
            DEBUG_ERROR_OUT("error:Operation::TryCycleMarkCheck TriggerAlways > 2S "
                            << "find=" << find
                            << ",err_msg=" << err_msg

            );
            SetLocMissEvent(err_msg, true);
            return false;
        }
    }

    DEBUG_OUT("TryCycleMarkCheck:cycle..find=" << find);
    return find;
}
